
#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <time.h>

#include "ibm2d.h"
#include "util.h"
#include "nsmg.h"
#include "boundary.h"
#include "dlist.h"
#include "output.h"

#define REAL double

//void indicator(REAL **bd_posi, REAL **bd_force, int nt)     // not used now !!!
//{
//	extern REAL h;
//
//	int  k;
//	REAL a, b;
//
//	for (k = 1; k <= nt; k++) {
//
//		a = bd_posi[k + 1][1] - bd_posi[k][1];
//		b = bd_posi[k + 1][2] - bd_posi[k][2];
//
//		bd_force[k][1] = -b / sqrt(a*a + b * b);
//		bd_force[k][2] = a / sqrt(a*a + b * b);
//	}
//
//	for (k = 1; k <= nt; k++) {
//		bd_force[k][1] *= -h;
//		bd_force[k][2] *= -h;
//	}
//}

void sf_force(REAL **fxx, REAL **fyy, REAL **bdpo, int nt, REAL *ds)
{
	extern REAL We;
	REAL **bd_force;
	REAL rWe;
	int i, j;

	rWe = 1.0 / We;

	bd_force = dmatrix(1, nt, 1, 2);

	boundary_force(bdpo, bd_force, nt, ds);
	fluid_force(fxx, fyy, bdpo, bd_force, nt);

	i0jloop{
		fxx[i][j] = fxx[i][j] * rWe;
	}

	ij0loop{
		fyy[i][j] = fyy[i][j] * rWe;
	}

	free_dmatrix(bd_force, 1, nt, 1, 2);
}



//void sf_force(REAL **fxx, REAL **fyy, BN * head)   // list structure, finished
//{
//	extern REAL We;
//	REAL **bd_force;
//	REAL rWe;
//	int i, j;
//
//	rWe = 1.0 / We;
//
//	bd_force = dmatrix(1, nt, 1, 2);
//
//	boundary_force(head);
//	fluid_force(head, fxx, fyy);
//
//	i0jloop{
//		fxx[i][j] = fxx[i][j] * rWe;
//	}
//
//		ij0loop{
//			fyy[i][j] = fyy[i][j] * rWe;
//	}
//
//	free_dmatrix(bd_force, 1, nt, 1, 2);
//}



void full_step(REAL **u, REAL **v, REAL **nu, REAL **nv, REAL **p, REAL **fxx, REAL **fyy)
{
	extern int nx, ny;

	REAL **adv_u, **adv_v, **tu, **tv;

	adv_u = dmatrix(0, nx, 1, ny);
	adv_v = dmatrix(1, nx, 0, ny);
	tu = dmatrix(0, nx, 1, ny);
	tv = dmatrix(1, nx, 0, ny);

	advection_step(u, v, adv_u, adv_v);

	temp_uv(tu, tv, u, v, adv_u, adv_v, fxx, fyy);

	Poisson(tu, tv, p);

	update_uv(nu, nv, tu, tv, p);


	free_dmatrix(adv_u, 0, nx, 1, ny);
	free_dmatrix(adv_v, 1, nx, 0, ny);
	free_dmatrix(tu, 0, nx, 1, ny);
	free_dmatrix(tv, 1, nx, 0, ny);
}

void update_uv(REAL **nu, REAL **nv, REAL **tu, REAL **tv, REAL **p)
{
	extern int nx, ny;
	extern REAL dt;

	int i, j;
	REAL **worku, **workv;

	worku = dmatrix(0, nx, 1, ny);
	workv = dmatrix(1, nx, 0, ny);

	grad_p(p, worku, workv, nx, ny);

	i0jloop{

		nu[i][j] = tu[i][j] - dt * worku[i][j];
	}

		ij0loop{

			nv[i][j] = tv[i][j] - dt * workv[i][j];
	}

	free_dmatrix(worku, 0, nx, 1, ny);
	free_dmatrix(workv, 1, nx, 0, ny);
}


void advection_step(REAL **u, REAL **v, REAL **adv_u, REAL **adv_v)
{
	extern int nx, ny;
	extern REAL h;

	int i, j;

	augmenuv(u, v, nx, ny);

	i0jloop{

		if (u[i][j] > 0.0)
			adv_u[i][j] = u[i][j] * (u[i][j] - u[i - 1][j]) / h;
		else
			adv_u[i][j] = u[i][j] * (u[i + 1][j] - u[i][j]) / h;

		if (v[i][j - 1] + v[i + 1][j - 1] + v[i][j] + v[i + 1][j] > 0.0)
			adv_u[i][j] += 0.25*(v[i][j - 1] + v[i + 1][j - 1] + v[i][j] + v[i + 1][j])*(u[i][j] - u[i][j - 1]) / h;
		else
			adv_u[i][j] += 0.25*(v[i][j - 1] + v[i + 1][j - 1] + v[i][j] + v[i + 1][j])*(u[i][j + 1] - u[i][j]) / h;
	}

		ij0loop{

			if (u[i - 1][j] + u[i][j] + u[i - 1][j + 1] + u[i][j + 1] > 0.0)
				adv_v[i][j] = 0.25*(u[i - 1][j] + u[i][j] + u[i - 1][j + 1] + u[i][j + 1])*(v[i][j] - v[i - 1][j]) / h;
			else
				adv_v[i][j] = 0.25*(u[i - 1][j] + u[i][j] + u[i - 1][j + 1] + u[i][j + 1])*(v[i + 1][j] - v[i][j]) / h;

			if (v[i][j] > 0.0)
				adv_v[i][j] += v[i][j] * (v[i][j] - v[i][j - 1]) / h;
			else
				adv_v[i][j] += v[i][j] * (v[i][j + 1] - v[i][j]) / h;
	}
}

void temp_uv(REAL **tu, REAL **tv, REAL **u, REAL **v, REAL **adv_u, REAL **adv_v, REAL **fxx, REAL **fyy)
{
	extern int nx, ny;
	extern REAL h, Re, dt;

	int i, j;
	REAL tmp_u, tmp_v;

	i0jloop{
		tmp_u = u[i + 1][j] + u[i - 1][j] - 4.0*u[i][j] + u[i][j + 1] + u[i][j - 1];
		tu[i][j] = u[i][j] + dt * (tmp_u / (Re*h*h) - adv_u[i][j] + fxx[i][j]);
	}

	ij0loop{
			tmp_v = v[i + 1][j] + v[i - 1][j] - 4.0*v[i][j] + v[i][j + 1] + v[i][j - 1];
			tv[i][j] = v[i][j] + dt * (tmp_v / (Re*h*h) - adv_v[i][j] + fyy[i][j]);
	}
}

void Poisson(REAL **tu, REAL **tv, REAL **p)
{
	extern int nx, ny;
	REAL **workp;

	workp = dmatrix(1, nx, 1, ny);

	source_p(tu, tv, workp, nx, ny);
	MG_Poisson(p, workp);

	free_dmatrix(workp, 1, nx, 1, ny);
}

void MG_Poisson(REAL **p, REAL **f)
{
	extern int nx, ny;

	int i, j, max_it, iter;
	REAL tol, residual, **tmp, **err;

	max_it = 2000;
	iter = 0;

	residual = 1.0;
	tol = 1.0e-5;

	err = dmatrix(1, nx, 1, ny);
	tmp = dmatrix(1, nx, 1, ny);
	mat_copy(tmp, p, 1, nx, 1, ny);

	while (iter < max_it && residual > tol) {

		vcycle_p(p, f, nx, ny, 1);
		pressure_update(p);

		ijloop{
			err[i][j] = tmp[i][j] - p[i][j];
		}

		residual = mat_max(err, 1, nx, 1, ny);
		mat_copy(tmp, p, 1, nx, 1, ny);

		iter++;
	}

	printf(" Mac iteration = %d  residual = %e \n\n", iter, residual);

	free_dmatrix(tmp, 1, nx, 1, ny);
	free_dmatrix(err, 1, nx, 1, ny);
}

void vcycle_p(REAL **uf, REAL **ff, int nxf, int nyf, int ilevel)
{
	extern int n_level;

	relax(uf, ff, nxf, nyf);

	if (ilevel < n_level)
	{
		int nxc, nyc;
		REAL **rf, **uc, **fc;

		nxc = nxf / 2;
		nyc = nyf / 2;

		rf = dmatrix(1, nxf, 1, nyf);
		uc = dmatrix(1, nxc, 1, nyc);
		fc = dmatrix(1, nxc, 1, nyc);

		residual(rf, uf, ff, nxf, nyf);
		restriction(rf, fc, nxc, nyc);
		zero_matrix(uc, 1, nxc, 1, nyc);

		vcycle_p(uc, fc, nxc, nyc, ilevel + 1);

		prolongation(uc, rf, nxc, nyc);
		mat_add(uf, uf, rf, 1, nxf, 1, nyf);

		relax(uf, ff, nxf, nyf);

		free_dmatrix(rf, 1, nxf, 1, nyf);
		free_dmatrix(uc, 1, nxc, 1, nyc);
		free_dmatrix(fc, 1, nxc, 1, nyc);
	}
}

void relax(REAL **p, REAL **f, int nxt, int nyt)
{
	extern int p_relax;
	extern REAL xright, xleft;

	int i, j, iter;
	REAL ht, ht2, coef, src;

	ht = (xright - xleft) / (REAL)nxt;
	ht2 = pow(ht, 2);

	for (iter = 1; iter <= p_relax; iter++)
	{
		ijloopt{
			coef = -2.0 / ht2;
			src = f[i][j];

			if (i == 1) {
				src -= (p[2][j] + p[nxt][j]) / ht2;
			}

			else if (i == nxt) {
				src -= (p[1][j] + p[nxt - 1][j]) / ht2;
			}

			else {
				src -= (p[i + 1][j] + p[i - 1][j]) / ht2;
			}

			if (j == 1) {
				src -= p[i][2] / ht2;
				coef += -1.0 / ht2;
			}

			else if (j == nyt) {
				src -= p[i][nyt - 1] / ht2;
				coef += -1.0 / ht2;
			}

			else {
				src -= (p[i][j + 1] + p[i][j - 1]) / ht2;
				coef += -2.0 / ht2;
			}

			p[i][j] = src / coef;
		}
	}
}


void restriction(REAL **uf, REAL **uc, int nxt, int nyt)
{
	int i, j;
	ijloopt{
		uc[i][j] = 0.25*(uf[2 * i - 1][2 * j - 1] + uf[2 * i - 1][2 * j] + uf[2 * i][2 * j - 1] + uf[2 * i][2 * j]);
	}

}


void prolongation(REAL **uc, REAL **uf, int nxt, int nyt)
{
	int i, j;
	ijloopt{
		uf[2 * i - 1][2 * j - 1] = uf[2 * i - 1][2 * j] = uf[2 * i][2 * j - 1] = uf[2 * i][2 * j] = uc[i][j];
	}

}


void grad_p(REAL **p, REAL **dpdx, REAL **dpdy, int nxt, int nyt)
{
	extern REAL xright, xleft;

	int i, j;
	REAL ht;

	ht = (xright - xleft) / (REAL)nxt;

	i0jloopt{

		if (i == 0)
			dpdx[0][j] = (p[1][j] - p[nxt][j]) / ht;

		else if (i == nxt)
			dpdx[nxt][j] = (p[1][j] - p[nxt][j]) / ht;

		else
			dpdx[i][j] = (p[i + 1][j] - p[i][j]) / ht;
	}

		ij0loopt{

			if (j == 0)
				dpdy[i][0] = 0.0;

			else if (j == nyt)
				dpdy[i][nyt] = 0.0;

			else
				dpdy[i][j] = (p[i][j + 1] - p[i][j]) / ht;

	}

}


void source_p(REAL **tu, REAL **tv, REAL **divuv, int nxt, int nyt)
{
	extern REAL dt;

	int i, j;

	div_uv(tu, tv, divuv, nxt, nyt);

	ijloopt{
		divuv[i][j] /= dt;
	}

}


void div_uv(REAL **tu, REAL **tv, REAL **divuv, int nxt, int nyt)
{
	extern REAL xright, xleft;

	int i, j;
	REAL ht;

	ht = (xright - xleft) / (REAL)nxt;

	ijloopt{
		divuv[i][j] = (tu[i][j] - tu[i - 1][j] + tv[i][j] - tv[i][j - 1]) / ht;
	}
}



void residual(REAL **r, REAL **u, REAL **f, int nxt, int nyt)
{
	laplace(u, r, nxt, nyt);
	mat_sub(r, f, r, 1, nxt, 1, nyt);
}


void laplace(REAL **p, REAL **lap_p, int nxt, int nyt)
{
	REAL **dpdx, **dpdy;

	dpdx = dmatrix(0, nxt, 1, nyt);
	dpdy = dmatrix(1, nxt, 0, nyt);

	grad_p(p, dpdx, dpdy, nxt, nyt);
	div_uv(dpdx, dpdy, lap_p, nxt, nyt);

	free_dmatrix(dpdx, 0, nxt, 1, nyt);
	free_dmatrix(dpdy, 1, nxt, 0, nyt);

}


void pressure_update(REAL **a)
{
	extern int nx, ny;

	int i, j;
	REAL ave = 0.0;

	for (i = 1; i <= nx; i++) {
		for (j = 1; j <= ny; j++) {

			ave = ave + a[i][j];
		}
	}
	ave /= (REAL)(nx*ny);


	for (i = 1; i <= nx; i++) {
		for (j = 1; j <= ny; j++) {

			a[i][j] -= ave;

		}
	}

	return;
}



void augmenuv(REAL **u, REAL **v, int nx, int ny)
{
	extern REAL velocity_boundary;

	int i, j;
	REAL bdvel = velocity_boundary;

	for (i = -1; i <= nx + 1; i++) {
		u[i][0] = -2.0*bdvel - u[i][1];
		u[i][ny + 1] = 2.0*bdvel - u[i][ny];
	}

	for (j = 0; j <= ny; j++) {

		u[-1][j] = u[nx - 1][j];
		u[nx + 1][j] = u[1][j];
	}

	for (i = 0; i <= nx; i++) {

		v[i][-1] = -v[i][1];
		v[i][ny + 1] = -v[i][ny - 1];

	}
	for (j = -1; j <= ny + 1; j++) {

		v[0][j] = v[nx][j];
		v[nx + 1][j] = v[1][j];
	}

}







